/*
 *  run3a.cpp
 *  FaceTracker
 *
 *  Created by Kenn Sebesta on 2008-04-24.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "bandpIncludes.h"
#include "findBallState.h"
#include "findBallOrientation.h"
#include "correctGamma.h"
#include "createCamera.h"
#include "displayOpenCV.h"
#include "guiControl.h"
#include "imageCropper.h"
#include "kalmanAdvanced.h"
#include "processImage.h"
#include "run.h"
#include "sendOutput.h"
#include "socketServer.h"
#include "updateSetPoint.h"
#include <time.h>
#include <sys/time.h>

//#define ACTUATOR_SATURATION 0.015 //LEGO TABLE PARAMETER
#define ACTUATOR_SATURATION 0.015
#define BALL_GONE_SECS 0.25
#define NUMCONTROLVALS  8
#define NUMMATRIXVALS   3000 //THIS NEEDS TO CHANGE!!!
#define PORT            5010
#define DATAPORT        5011
#define CALIBRATION_FILE "calibrationTables.yml"
#define USER_VARIABLES "bandpUserVariables_3.yml"
#define SYSTEM_VARIABLES "bandpSystemVariables_3.yml"
#define TRAJECTORY_DATA "bandpTrajectoryData.yml"
#define SAVED_PAST 5 // Number of seconds of previous inputs and outputs we keep in memory
#define STATE_DIM 16 // Number of state dimensions
using namespace std;

extern  char *WINDOW_RUN; // Is defined a second time in displayOpenCV. Needs to be global instead.
extern  char *WINDOW_ADJUST;
extern char tbarname1[];
extern char tbarname2[];

extern double kt;
extern double g;
extern int viewSize;
extern int orientBall; // This turns on and off ball orientation. This should be modifiable at run time, not just at compilation time.
extern float decimate;
//double 	convThresh=4e06;// THIS VALUE IS MUCH TOO HIGH, I FEEL.
extern double 	convThresh;
const double ticRate=cvGetTickFrequency()*1e06;
extern double delT;
extern double calibration;
extern  char *cameraLib; //Chooses camera control library. Possible choices are "camwire" and "opencv"
extern TableCalibration calLUT[3]; //Calibration lookup tables for actuator control
extern CvMat *levelCalibration;
extern CvRect limitR;
//double ballDia;
extern SystemDims table;

int run3a( int gui ) {
    cvNamedWindow( WINDOW_RUN, CV_WINDOW_AUTOSIZE );
    cvNamedWindow( WINDOW_ADJUST, CV_WINDOW_AUTOSIZE );
    cvMoveWindow( WINDOW_RUN, 320, 250);
    
//------------------------------BEGIN 1ST SANDBOX-------------------------//

    CvFileStorage* trajectoryData = cvOpenFileStorage( TRAJECTORY_DATA, 0, CV_STORAGE_READ );
    CvMat *customTrajectory=(CvMat*)cvReadByName( trajectoryData, NULL, "square" );
    cvReleaseFileStorage( &trajectoryData );

    
    bool observerOnOff=1;
    
    //Open file for data out
    FILE *dataOut;
    dataOut = fopen("dataOut.cvs", "w+");
    fprintf(dataOut, "x(1)     vx(2)    y(3)     vy(4)     s.p.(1) s.p.(2) s.p.(3) s.p.(4) y(1)    y(2)  \t\tu(1)      u(2)\n");
    
    //Data for automatically correcting image distortion
    //TAKES TOO LONG. DEACTIVATED!!!
    CvMat *intrinsic_matrix=cvCreateMatHeader( 3, 3, CV_32F ), *distortion_coeffs=cvCreateMatHeader( 4, 1, CV_32F );
    float intM[]={1240.771, 0.0, 561.1526, 0.0, 1263.671, 640.2196, 0.0, 0.0, 1.0};
    float distCoef[]={2.428849e-02, 1.703325e-01, 4.398058e-02, -8.397999e-03};
    cvInitMatHeader( intrinsic_matrix, 3, 3, CV_32F, intM );
    cvInitMatHeader( distortion_coeffs, 4, 1, CV_32F, distCoef );
    
    
    
    
    //Observer for complete system state, including ball orientation and actuator height
    CvKalman *kalman_system = cvCreateKalman(16, 11, 3);
//    CvMat ballOrientation;
//    cvGetSubRect( kalman_system->state_post, &ballOrientation, cvRect( 0, 8, 0, 13 ) );
//    CvMat *orientation_measrd=cvCreateMat(7, 1, CV_32F);
    
    cvSetIdentity( kalman_system->measurement_noise_cov );
    cvSetIdentity( kalman_system->error_cov_post );

//    CvMat *y_measrdR_system=cvCreateMat( 11, 1, CV_32F ); cvZero( y_measrdR_system );
//    cvmSet( y_measrdR_system, 5, 0, 1 );
//    cvmSet( y_measrdR_system, 8, 0, .1 );
//    cvmSet( y_measrdR_system, 9, 0, 0 );
//    cvmSet( y_measrdR_system, 10, 0, 1 );



    CvMat *x_hat=cvCreateMat( 16, 1, CV_32F );
    CvMat *x_hatR=kalman_system->state_post;
    CvMat *y_measrd=cvCreateMat( 11, 1, CV_32F ); //All 
    CvMat *y_measrdR=cvCloneMat( y_measrd );
    cvmSet( y_measrdR, 9, 0, 0 );
    cvmSet( y_measrdR, 10, 0, 1 );
    
    CvMat y_measrd_xy, y_measrdR_xy, *y_measrd_ijk, y_measrdR_ijk;
    y_measrd_ijk=cvCreateMat( 4, 1, CV_32F ); cvZero(y_measrd_ijk);
    cvGetSubRect( y_measrd, &y_measrd_xy, cvRect(0, 0, 1, 2) );
    cvGetSubRect( y_measrd, y_measrd_ijk, cvRect(0, 5, 1, 4) );
    cvGetSubRect( y_measrdR, &y_measrdR_xy, cvRect(0, 0, 1, 2) );    
    cvGetSubRect( y_measrdR, &y_measrdR_ijk, cvRect(0, 5, 1, 4) );
    
    //Initial ball orientation. COMPLETE GUESS. DON'T EVEN KNOW IF NECESSARY
    cvmSet( &y_measrdR_ijk, 0, 0, 1 );
    cvmSet( &y_measrdR_ijk, 1, 0, 0 );
    cvmSet( &y_measrdR_ijk, 2, 0, 0 );
    cvmSet( &y_measrdR_ijk, 3, 0, 0.1 );


    
    CvMat x_hat_xy, x_hatR_xy, x_hat_ijk, x_hatR_ijk;
    cvGetSubRect( x_hat, &x_hat_xy, cvRect( 0, 0, 1, 4 ) );
    cvGetSubRect( x_hat, &x_hat_ijk, cvRect( 0, 8, 1, 6 ) );
    cvGetSubRect( kalman_system->state_post, &x_hatR_xy, cvRect( 0, 0, 1, 4 ) );
    cvGetSubRect( kalman_system->state_post, &x_hatR_ijk, cvRect( 0, 8, 1, 6 ) );
    
    //Observer for ball's orientation
//    CvKalman *kalman_orientation = cvCreateKalman(9, 7, 0);

//    cvZero( orientation_measrd );
//    cvmSet( orientation_measrd, 5, 0, 1);
//    cvmSet( orientation_measrd, 6, 0, 1);
//    
//    
//    double wx, wy, wxwx, wywy, xbd=0.000000001, ybd=0.000000001, xb, yb;
//    CvMat* Kpk=cvCreateMat(9, 7, CV_32F);
//    CvMat* Pk=cvCreateMat(9, 9, CV_32F);
//    CvMat* Pkm=cvCreateMat(9, 9, CV_32F);
//    CvMat* Pk_old=cvCreateMat(9, 9, CV_32F);
//    CvMat* ballOrientation=cvCreateMat(9, 1, CV_32F);
//    CvMat* ballOrientationA=cvCreateMat(9, 9, CV_32F);
//    CvMat* ballOrientationC=cvCreateMat(7, 9, CV_32F);
//    cvZero( ballOrientation );
//    cvZero( ballOrientationA );
//    cvZero( ballOrientationC );
//    cvSet( Pk, cvScalar(1) );
//    
//    cvmSet( ballOrientation, 0, 0, 1 );
//    cvmSet( ballOrientation, 5, 0, 1 );
//    cvmSet( ballOrientationC, 0, 0, 1 );
//    cvmSet( ballOrientationC, 1, 1, 1 );
//    cvmSet( ballOrientationC, 2, 3, 1 );
//    cvmSet( ballOrientationC, 3, 4, 1 );
//    cvmSet( ballOrientationC, 4, 6, 1 );
//    cvmSet( ballOrientationC, 5, 7, 1 );
//    cvmSet( ballOrientationC, 6, 8, 1 );
//    
//    CvMat *ballOrientationQ=cvCreateMat( 9, 9, CV_32F );
//    CvMat *ballOrientationR=cvCreateMat( 7, 7, CV_32F );
//    cvSetIdentity( ballOrientationQ, cvScalar(1) );
//    cvSetIdentity( ballOrientationR );
//    
//    
//    kalman_orientation->transition_matrix=ballOrientationA;
//    kalman_orientation->measurement_matrix=ballOrientationC;
//    kalman_orientation->process_noise_cov=ballOrientationQ;
//    kalman_orientation->measurement_noise_cov=ballOrientationR;
//    kalman_orientation->state_post=ballOrientation;
//    kalman_orientation->error_cov_pre=Pk;
//    cvCopy(kalman_orientation->error_cov_pre, kalman_orientation->error_cov_post);
//
////    cvZero( kalman_orientation->state_pre );
    
    double xi, yi, zi, xk, yk, zk;        
    CvMat *Rot=cvCreateMat(3, 3, CV_32F);
    CvMat *zVector=cvCreateMat(3, 1, CV_32F);
    cvZero(zVector);
    cvmSet(zVector, 2, 0, -1);
    CvMat *tmp33=cvCreateMat(3,3,CV_32F);
    
//------------------------------END 1ST SANDBOX-------------------------//
    
    // Create matrices storing previous points.
    CvMat tmpSavedPast;
    CvMat *savedPast=cvCreateMat((int)(SAVED_PAST/0.02), STATE_DIM, CV_32F );
    CvMat *advanceTime=cvCreateMat(savedPast->rows, savedPast->rows, CV_32F);
    cvZero(advanceTime); cvZero(savedPast);
    cvGetSubRect(advanceTime, &tmpSavedPast, cvRect(1, 0, advanceTime->cols-1, advanceTime->rows-1));
    cvSetIdentity(&tmpSavedPast);
    cvGetSubRect( savedPast, &tmpSavedPast, cvRect( 0, savedPast->rows-1, savedPast->cols, 1 ) );
    
    
    //==============================================
    // Load config file
    //==============================================
    double ballSpeedR;
//    CvKalman *kalman_ball = cvCreateKalman(4, 2, 2);
    SetPoint setpoint; // Declare setpoint structure.
    
    CvFileStorage* userVariables = cvOpenFileStorage( USER_VARIABLES, 0, CV_STORAGE_READ );
    delT = cvReadRealByName( userVariables, NULL, "delT" );
    table.ballDia=cvReadRealByName( userVariables, NULL, "ballDia");
    table.cameraHeight=cvReadRealByName( userVariables, NULL, "cameraHeight", 0);
    table.actuator_0=cvCreateMat(1,2, CV_32F), cvZero( table.actuator_0 );
    table.actuator_1=(CvMat*)cvReadByName( userVariables, NULL, "actuator_1" );
    table.actuator_2=(CvMat*)cvReadByName( userVariables, NULL, "actuator_2" );
    setpoint.integrationPositionGain= cvReadRealByName( userVariables, NULL, "integrationPositionGain" );
    setpoint.integrationVelocityGain= cvReadRealByName( userVariables, NULL, "integrationVelocityGain" );
    setpoint.integrationPositionErr=(CvMat*)cvReadByName( userVariables, NULL, "integrationPositionErr" );
    setpoint.integrationVelocityErr=(CvMat*)cvReadByName( userVariables, NULL, "integrationVelocityErr" );
    setpoint.ballSpeedR = cvReadRealByName( userVariables, NULL, "ballSpeedR" );
    ballSpeedR=setpoint.ballSpeedR;
    
    strcpy( setpoint.pathName, (char*)cvReadStringByName( userVariables, NULL, "pathName" ) );	// Thus, when cvReleaseFileSystem is called, the variable loses its value.   
    
    kalman_system->process_noise_cov=(CvMat*)cvReadByName( userVariables, NULL, "Q_kalman" );
    kalman_system->measurement_noise_cov=(CvMat*)cvReadByName( userVariables, NULL, "R_kalman" );
    setpoint.tableCenter=(CvMat*)cvReadByName( userVariables, NULL, "tableCenter" );
    
    //HASN'T BEEN INCLUDED YET IN CONFIG FILE
    double sat=ACTUATOR_SATURATION; //Saturation for actuators
    int gammaCorrect=GAMMA_ON;
    
    
    double thetaGG=cvReadRealByName( userVariables, NULL, "thetaGG", 1);
    CvMat *deltaTheta;
    deltaTheta=cvCreateMat( 16, 16, CV_32F );
    for (int i=0; i<16; i++){
        cvmSet( deltaTheta, i, i, pow(thetaGG, i));
    }
    
    cvScale( kalman_system->process_noise_cov, kalman_system->process_noise_cov, pow(thetaGG, 2) );
    cvMatMul( kalman_system->process_noise_cov, deltaTheta, kalman_system->process_noise_cov);
    cvMatMul( deltaTheta, kalman_system->process_noise_cov, kalman_system->process_noise_cov);
    
    // Local Garbage collection
    cvReleaseFileStorage( &userVariables );
    cvReleaseMat( &deltaTheta );
    
    printf("Done loading user variables.\n");
    
    
    //==============================================
    // Load calibration files
    //==============================================
    CvFileStorage* calTables = cvOpenFileStorage( CALIBRATION_FILE, 0, CV_STORAGE_READ );
    
    // Load actuator calibration data
    calLUT[0].lut = (CvMat*)cvReadByName( calTables, NULL, "calActuator_0" );
    calLUT[1].lut = (CvMat*)cvReadByName( calTables, NULL, "calActuator_1" );
    calLUT[2].lut = (CvMat*)cvReadByName( calTables, NULL, "calActuator_2" );
    
    // Load camera gamma calibration data
    levelCalibration = (CvMat*)cvReadByName( calTables, NULL, "levelCalibration" );
    
    // Load camera gamma calibration data...
    CvMat *cameraGammaData, *cameraGammaCurveFit;
    cameraGammaData= (CvMat*)cvReadByName( calTables, NULL, "cameraGammaData" );
    cameraGammaCurveFit= (CvMat*)cvReadByName( calTables, NULL, "cameraGammaCurveFit" );
    // ... and then generate gamma correction look-up table
    CvMat *cameraGammaLUT=cvCreateMat( 256, 1, CV_8U );
    cvZero(cameraGammaLUT);
    createGammaLUT( cameraGammaData, cameraGammaCurveFit, &cameraGammaLUT );
    
    // Local Garbage collection
    cvReleaseFileStorage( &calTables );
    cvReleaseMat( &cameraGammaData );
    cvReleaseMat( &cameraGammaCurveFit );
    
#ifdef DEBUG
    printf("Done loading calibration file.\n");
#endif
    
    
    //==============================================
    // Load generated data file
    //==============================================
    IplImage *maskImage;
    CvMat *K_dlqr_3;
    ImageAnalysisParams params;
    
    
    CvFileStorage* systemVariables = cvOpenFileStorage( SYSTEM_VARIABLES, 0, CV_STORAGE_READ);
    params.binaryThresh = cvReadIntByName( systemVariables, NULL, "binaryThresh");
    params.cannyThreshLow = cvReadIntByName( systemVariables, NULL, "cannyThreshLow");
    params.cannyThreshHigh = cvReadIntByName( systemVariables, NULL, "cannyThreshHigh");
    calibration = cvReadRealByName( systemVariables, NULL, "calibration");
//    kalman_system->transition_matrix=(CvMat*)cvReadByName( systemVariables, NULL, "A_dscrt" );
//    kalman_system->control_matrix=(CvMat*)cvReadByName( systemVariables, NULL, "B_dscrt" );
//    kalman_system->measurement_matrix=(CvMat*)cvReadByName( systemVariables, NULL, "C" );
//    kalman_system->state_post=(CvMat*)cvReadByName( systemVariables, NULL, "x_hatR" );
    kalman_system->error_cov_post=(CvMat*)cvReadByName( systemVariables, NULL, "PkCovMat" );
    K_dlqr_3=(CvMat*)cvReadByName( systemVariables, NULL, "K_dlqr_3" );
    maskImage=(IplImage*)cvReadByName( systemVariables, NULL, "maskImage" );
    
    // Check hash
    //NEEDS TO BE WRITTEN.
#ifdef DEBUG
    // Stupidity check
    std::cout << "pathName is  " << setpoint.pathName << "\n";
    std::cout << "cannyThreshHigh is  " << params.cannyThreshHigh << "\n";
    std::cout << "cannyThreshLow is  " << params.cannyThreshLow << "\n";
    std::cout << "calibration is  " << calibration << "\n";
//    std::cout << "\nA_dscrt is \n";
//    printCvArr( kalman_system->transition_matrix );
//    std::cout << "\nB_dscrt is \n";
//    printCvArr( kalman_system->control_matrix );
//    std::cout << "\nC_dscrt is \n";
//    printCvArr( kalman_system->measurement_matrix );
    std::cout << "\nx_hatR is \n";
    printCvArr( kalman_system->state_post );
    std::cout << "\nQ_kalman is \n";
    printCvArr( kalman_system->process_noise_cov );
    std::cout << "\nR_kalman is \n";
    printCvArr( kalman_system->measurement_noise_cov );
    std::cout << "\nPkm is \n";
    printCvArr( kalman_system->error_cov_post );
    std::cout << "\nK_dlqr_3 is \n";
    printCvArr( K_dlqr_3 );
#endif
    // Local Garbage collection
    cvReleaseFileStorage( &systemVariables );
    
#ifdef DEBUG
    printf("Done loading system variables.\n\n");
#endif
    
    
    //==============================================
    // Generate ball mask and load markers outline file
    //==============================================
    MaskObject *markers, ball;
    
//    ball.image=maskImage;
//    ball.image=(IplImage*)cvClone(maskImage);
//    cvZero(ball.image);
    ball.dia=table.ballDia;
    ball.diaPix=maskImage->height;
    ball.state=&x_hat_xy;
    ball.pixCoordsMat=cvCreateMat( 2, 1, CV_32F ); //THIS IS THE ONLY ONE SO FAR EXPRESSED CORRECTLY. THE OTHER MATRICES NEED TO BE THUSLY DEFINED
//    cvCircle( ball.image, cvPoint((ball.image->width-1)/2, (ball.image->height-1)/2), ball.dia*calibration/2-3, cvScalar(255, 255, 255), -1);
    //Assigns the pixCoords and pixCoordsMat to the same data set.
    //This is useful in the program because sometimes it's far easier
    //to access one or the other.
    cvInitMatHeader( ball.pixCoordsMat, 2, 1, CV_32F, ball.pixCoords );
    int numMarkers=0;
    if ( orientBall )
        loadMarkers(&markers, &numMarkers);
    
#ifdef DEBUG
    printf("Done loading markers.\n");
#endif
    
    //==============================================
    // Open ports (both Signal Output and TCP Socket)
    //==============================================
    
    //Serial ports are OS dependent.  Might consider eventual migration to GNU Common C++, which is multiplatform
    int fd = openPort();
#ifdef SERIAL
    char szBuf[22];
    sprintf(szBuf, "#12 P1500 #28 P15000\r");
    
    if( !write(fd, szBuf, 22) ) //This command serves only to initialize the SSC32 card. I use the 50Hz pulses from the SSC-32 to trigger the camera.
        fputs("write() of 21 bytes failed!\n", stderr);
#endif
    
    
#ifdef GUI
    //Initialize client class for Java/C TCP socket
    Server bandpGUILink(PORT, DATAPORT);
    if(gui){
        printf("Please load bandpGUI.jar now. Waiting...");
        // Connect to client
        bandpGUILink.connect();
    }
#endif
    
#ifdef DEBUG
    printf( "Ports initialized.\n\n" );
#endif
    
    
    //==============================================
    // Set up camera
    //==============================================
    CamHandle camFeed; //Structure that includes OpenCV or Camwire camera instructions
    IplImage *camFrame, *camFrameUncorrected, *colorImage ;
    ImageSet images;
    CvMat *cameraCenter=cvCreateMat( 2, 1, CV_32F ); //This is where the camera is located relative to the image center.
    cvmSet(cameraCenter, 0, 0, 240); //THESE SHOULD BE AUTOMATICALLY GENERATED
    cvmSet(cameraCenter, 1, 0, 281); //THESE SHOULD BE AUTOMATICALLY GENERATED
    params.isDifferent=-1; //Since no images defined, isDifferent must be less then zero in order not to crash in processImage
    
//#ifdef CAMWIRE
//    if(!strcmp(cameraLib, "camwire"))
    camFrame=cvCreateImage( cvSize(480, 480), IPL_DEPTH_8U, 1 ); //NEEDS SIZE TO BE AUTOMATICALLY CONFIGURED. How?   
    camFrameUncorrected=cvCreateImage( cvGetSize(camFrame), camFrame->depth, camFrame->nChannels ); //NEEDS SIZE TO BE AUTOMATICALLY CONFIGURED. How?   
//#endif
    
    setupCamera( &camFeed );            //Sets up camera
    queryCamera( &camFrame, camFeed );  //Returns image
    
    if ( !camFrame ) {
        fprintf(stderr, "ERROR: queryCamera is returning null. Exiting...\n");
        exit(1);
    }
    
    colorImage = cvCreateImage( cvGetSize( camFrame ), IPL_DEPTH_8U, 3 );
   
#ifdef DEBUG
    printf( "Camera initialized.\n\n" );
#endif
    
    
    //==============================================
    // Initialize loop matrices and variables
    //==============================================
    IplImage *edgeImageOUT;
    CvMat *u_3=cvCreateMat( 3, 1, CV_32F ); cvZero(u_3);
    setpoint.errMat=cvCreateMat( 4, 1, CV_32F );
    CvMat* dft_mIm = cvCreateMat( 1, 1, CV_32F );
    CvMat *setPoint, *setPointR;
    limitR=cvRect( 0, 0, camFrame->width, camFrame->height );
    images.cropRect=cvRect( 0, 0, camFrame->width, camFrame->height );
    
    double minConv, maxConv, tmpSat;
    double *convMin, *convMax;
    convMin=&minConv;	convMax=&maxConv; //Necessary to assign pointer, or else things crash.
    int keyPress=-1, keyHold='1', ballGone=1, missingCount=1000, nthLoop=0;
    setpoint.pathNum=0;
//    int pathNum=setpoint.pathNum;
    int numMarkersFound=0, numOverruns=0;
    int64 tic_speed, tic_fps, toc, toc_fps;
    double computationTime, fpsRate;
    char filename[] = "./frame.bmp";			//filename for saving image
    viewSize=cvRound( maskImage->width+2*0.20*calibration*delT ); // viewSize is the viewport size, where 0.20 is the maximum speed in m/s that the ball will likely reach.
//    double ballSpeed=ballSpeedR*calibration/delT;
    bool displayConsole=true, startStop;
    setpoint.setPoint=cvCreateMat( 4, 1, CV_32F );	setpoint.setPointR=cvCreateMat( 4, 1, CV_32F );
//    setPoint=setpoint.setPoint;	setPointR=setpoint.setPointR;
    setpoint.ballSpeed=setpoint.ballSpeedR*calibration;
    CvFont textFont;
    cvInitFont( &textFont, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.6, 0, 1);
    char ballLocText[20];
    char fpsText[50];
    
    setpoint.pathRadius=80;
    double runTime, pathRadius=setpoint.pathRadius;
    int displayType;
//    float F[NUMMATRIXVALS+NUMCONTROLVALS]; //Create array that will receive matrix data from Java GUI
//    double tmpControlVals[NUMCONTROLVALS]; //Create array that will receive parameter data from Java GUI
    int numReadFromSocket=0;
    
    if(!gui){
        cvCreateTrackbar(tbarname1, WINDOW_ADJUST, &params.cannyThreshHigh, 300, NULL );
        cvCreateTrackbar(tbarname2, WINDOW_ADJUST, &params.cannyThreshLow, 300, NULL );
        startStop=1;
    }
    else{
        startStop=0;
        displayConsole=false;
    }
    
    
#ifdef DEBUG
    printf( "Variables initialized.\n\n" );
#endif

    //Initialize other variables that haven't found their home yet.
    int orientationLocks=0;
    ball.image=cvCreateImage(cvSize(viewSize, viewSize), IPL_DEPTH_8U, 1);
    cvZero(ball.image);
    cvCircle( ball.image, cvPoint(cvRound((viewSize-1)/2.0), cvRound((viewSize-1)/2.0)), cvRound(ball.dia*calibration/2-5), cvScalar(255, 255, 255), -1);
    
    CvMemStorage *contoursFoundStorage=cvCreateMemStorage(0);
    CvSeq *contoursFound=cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, /* sequence of integer elements */
                          sizeof(CvContour), /* header size - no extra fields */
                          sizeof(CvPoint), /* element size */
                          contoursFoundStorage /* the container storage */ );
    
    //=====================================================================================
    // PRELIMINARY LOOP
    //
    // This loop runs for a few moments in order to find the ball.
    // This ensures that the observer has converged and that the
    // the table has a smooth start.
    //=====================================================================================
    while( 1 ) {
        
        queryCamera( &camFrame, camFeed );
#ifdef CAMWIRE
        camwire_get_timestamp(camFeed.camwire, &params.timestamp);
        params.trigger_time = params.timestamp.tv_sec + 1.0e-9*params.timestamp.tv_nsec;
        gettimeofday(&params.tv_received, NULL);
        params.received_time = params.tv_received.tv_sec + 1e-6*params.tv_received.tv_usec;
        params.latency = params.received_time - params.trigger_time;
#endif
        
        if (gammaCorrect){
            cvLUT( camFrame, camFrame, cameraGammaLUT ); //Gamma correction
        }

        imageCropper( &images.cropRect, &y_measrd_xy, limitR, viewSize, missingCount, ballGone );
       
        // Processes image and extracts edges.
        processImage( camFrame, &images, &params);
        
        // Image analysis module that locates the ball.
        ballFinder( maskImage, camFrame, images, &dft_mIm, images.cropRect, params, //Inputs
                &missingCount, convMin, convMax, &y_measrd_xy, &images.convMat, &edgeImageOUT );	//Outputs
        cvScale( &y_measrd_xy, &y_measrdR_xy, 1/calibration );

        //Have we found the ball?
        if ( missingCount > (int)(BALL_GONE_SECS/delT) ) {
            ballGone=1;
//            printf("Warning, ball is missing for %d frames. Max conv. value: %4.3e.  BallGone: %d\n", missingCount, maxConv, ballGone);
        }
        else{
            ballGone=0;
            if (nthLoop <3){
//                cvmSet(x_hatR, 0, 0, cvmGet(y_measrdR, 0, 0));
//                cvmSet(x_hatR, 2, 0, cvmGet(y_measrdR, 1, 0));
            }
        }
        if( !ballGone && orientBall && (orientationLocks!=numMarkers) ){
            markerLocalizer( &contoursFound, images, ball, markers, numMarkers, &numMarkersFound, params );
//            printf("0rientationLock falied,  orientationLocks = %d\n", orientationLocks);
        }

        if(ballGone || !orientBall){
            for (int i=1; i<numMarkers; i++)
                markers[i].found=false;
            }
        else if (orientationLocks == numMarkers || numMarkersFound >= numMarkers ){
            orientationFinder( markers, images, &x_hatR_ijk /*observed variables*/, &y_measrd_xy, numMarkers, contoursFound, params,
                    &orientationLocks, &y_measrd_ijk);
            if (orientationLocks != numMarkers)
                orientationLocks=0;
        }        
        else{ //These trajectories move the ball around in order to find the three orientation markers. THEY NEED TO BE PROGRAMMED.
            printf("Warning, ball orientation is unknown. %d markers were found.\n", numMarkersFound);
            setpoint.pathNum=0;
            strcpy(setpoint.pathName, "findMarkers");
        }

        
        //==============================================
        // Observer module for reconstructing the ball's state
        //==============================================

        kalmanCD_3_predict( &kalman_system, u_3 );
        if (missingCount==0) //If we have found the ball, correct...
              kalmanCD_3_correct( &kalman_system, y_measrdR );
//            cvKalmanCorrect( kalman_system, y_measrdR );
        else
            cvCopy(kalman_system->state_pre, kalman_system->state_post);
        
        cvScale( &x_hatR_xy, &x_hat_xy, calibration );
        printCvArr( &x_hatR_xy );

        // Local garbage collection
        cvReleaseMat( &images.convMat );
        cvReleaseImage( &edgeImageOUT );

        fprintf(dataOut, "%5.5f, %5.5f, %5.5f, %5.5f, \t", cvmGet( x_hatR, 0, 0 ), cvmGet( x_hatR, 1, 0 ), cvmGet( x_hatR, 2, 0 ), cvmGet( x_hatR, 3, 0 ));
        fprintf(dataOut, "%5.1f, %5.1f, %5.1f, %5.1f, \t", cvmGet( setpoint.setPoint, 0, 0 ), cvmGet( setpoint.setPoint, 1, 0 ), cvmGet( setpoint.setPoint, 2, 0 ), cvmGet( setpoint.setPoint, 3, 0 ));
        fprintf(dataOut, "%6.2f, %6.2f, \t", cvmGet( y_measrd, 0, 0 ), cvmGet( y_measrd, 1, 0 ));
        fprintf(dataOut, "%f, %f\n", cvmGet( u_3, 0, 0 ), cvmGet( u_3, 1, 0 ));

        if(missingCount ==0 && nthLoop>5 && orientationLocks==numMarkers && orientBall){
            xi=cvmGet( &x_hatR_ijk, 0, 0 );
            yi=cvmGet( &x_hatR_ijk, 1, 0 );
            zi=cvmGet( &x_hatR_ijk, 2, 0 );
            xk=cvmGet( &x_hatR_ijk, 3, 0 );
            yk=cvmGet( &x_hatR_ijk, 4, 0 );
            zk=cvmGet( &x_hatR_ijk, 5, 0 );
            
            break;
        }
        else if (missingCount ==0 && nthLoop>5)
            break;

//***************************************************************//        
//        cvmSet( y_measrdR, 0, 0, .2 );
//        cvmSet( y_measrdR, 1, 0, .2 );
//        
//        printCvArr(y_measrdR);
//        printCvArr(u_3);
//        for (int i=0; i<500; i++)
//        {
////            kalmanCD( &(kalman_system->state_post), y_measrdR_system, u_system, &kalman_system );
//              kalmanCD_3_predict( &kalman_system, u_3 );
//              kalmanCD_3_correct( &kalman_system, y_measrdR );
//          }
//        
//        printCvArr(kalman_system->state_post);
//        printCvArr(kalman_system->error_cov_post);
//        exit(1);
//**************************************************************//        
        
        // Test for end of loop
        if(nthLoop>2/delT){
            printf("\nCannot find ball. Pausing...\n");
            do {
                queryCamera( &camFrame, camFeed );
                processImage( camFrame, &images, &params);
                cvShowImage( "Parameters", images.edgeImage );
                cvShowImage( "Ball andPlate", maskImage );
                 keyPress=( cvWaitKey(2) & 255 );
            }while( keyPress != 27 );
//            exit(1);
//            break;
        }
        nthLoop++;        
    }
    

    
    //=====================================================================================
    // MAIN LOOP
    //
    // Runs until user hits 'Esc'
    //=====================================================================================
    printf( "Program starting...\n" );
    keyPress=0;
    nthLoop=0;
    while( keyPress != 27 ) {
        
        //------------------------------BEGIN 2ND SANDBOX-------------------------//
        //------------------------------END 2ND SANDBOX---------------------------//
        
        
        
        // Keep track of time
        nthLoop++; //This is the nth loop so far...
        runTime=nthLoop*delT;
        cvMatMul( advanceTime, savedPast, savedPast ); //Updates matrix with most recent state values
        cvTranspose( x_hatR , &tmpSavedPast );

//        cvmSet( &tmpSavedPast, 0, 0, cvmGet(y_measrd,0,0)/calibration); //make output measured data, instead of filtered data
//        cvmSet( &tmpSavedPast, 0, 2, cvmGet(y_measrd,1,0)/calibration); //ONLY FOR DEVELOPMENT!!! THIS MUST BE REMOVED FOR PRODUCTION VERSION.
        
        //Get image and image metadata
        queryCamera( &camFrameUncorrected, camFeed );

        
#ifdef CAMWIRE
        double oldTime=params.trigger_time;
        gettimeofday(&params.tv_received, NULL);
        camwire_get_timestamp(camFeed.camwire, &params.timestamp);
        params.trigger_time = params.timestamp.tv_sec + 1.0e-9*params.timestamp.tv_nsec;
        params.received_time = params.tv_received.tv_sec + 1e-6*params.tv_received.tv_usec;
        params.latency = params.received_time - params.trigger_time;
        
        fpsRate=1/(params.trigger_time-oldTime); //Computes fps
#else
        toc_fps=cvGetTickCount();
        fpsRate=ticRate/(double)(toc_fps-tic_fps);
        tic_fps=cvGetTickCount();
#endif
        tic_speed=cvGetTickCount();
        

        //==============================================
        // Image pre-processing module. Corrects for lens distortion, light, and pinhole camera model .
        //==============================================

        if (0){//Undistorting takes too much time! Leave off until intelligent way of performing undistortion is found
            cvUndistort2( camFrameUncorrected, camFrame, intrinsic_matrix, distortion_coeffs );  
        }
        else
            camFrame=camFrameUncorrected;
        
//        cvEqualizeHist( camFrame, camFrame ); //THIS COULD BE INTERESTING, BUT TENDS NOT TO WORK WITH MY PARTICULAR APPLICATION. SHOULD BE USER CONFIGURABLE.
                                                //IN FACT, THE HISTORGRAM ALGORITHM AS SHOWN IN OPENCV DOCS SHUOLD BE APPLIED IN ORDER TO ALLOW FOR USER ADJUSTABLE BRIGHTNESS LEVELS.       
        if (gammaCorrect){
            cvLUT( camFrame, camFrame, cameraGammaLUT ); //Gamma correction
        }
        cvCvtColor( camFrame, colorImage, CV_GRAY2RGB ); //Copies input image to color version. The color version is used for output display.
        
#ifdef GUI
        //==============================================
        // Remote control input module.
        //==============================================
        if( gui && !((nthLoop-1) % cvRound(2/delT)) ){
#if defined DEBUG && (DEBUG > 5)
            printf("Reading TCP port...\n");
#endif
            guiIO(bandpGUILink, &kalman_system, &setpoint, &params, &K_dlqr_3, &startStop, &displayType, &missingCount,  &ballGone,  &keyHold);
            setpoint.ballSpeed=setpoint.ballSpeedR*calibration;         
        }
#endif
        
        
#if defined DEBUG && (DEBUG > 2)
        printf("Correcting image information...\n");
#endif  

        //==============================================
        // Image analysis module which finds the ball
        //==============================================
        
#if defined DEBUG && (DEBUG > 2)
        printf("Entering image cropper...\n");
#endif
        // Creates image cropping rectangle so the loop runs faster
        imageCropper( &images.cropRect, y_measrd, limitR, viewSize, missingCount, ballGone );
        
#if defined DEBUG && (DEBUG > 2)
        printf("Processing image...\n");
        toc = cvGetTickCount();
        printf("    Computation time: %3.3lf ms.\n", (double)(toc-tic_speed)*1000/ticRate);
#endif
        // Processes image and extracts edges.
        processImage( camFrame, &images, &params);
        
       
#if defined DEBUG && (DEBUG > 2)
        printf("Finding ball...\n");
#endif
        // Image analysis module that locates the ball.
        ballFinder( maskImage, camFrame, images, &dft_mIm, images.cropRect, params, //Inputs
                &missingCount, convMin, convMax, y_measrd, &images.convMat, &edgeImageOUT );	//Outputs
        
        //Have we found the ball?
        if ( missingCount > (int)(BALL_GONE_SECS/delT) ) {
            ballGone=1;
//            cvZero( setpoint.integrationPositionErr ); //This should reset to previously saved values, instead of 0
//            cvZero( setpoint.integrationVelocityErr ); //This should reset to previously saved values, instead of 0
//            cvCopy( setpoint.integrationPositionErr );
//            cvCopy( setpoint.integrationVelocityErr );
            printf("Warning, ball is missing for %d frames. Max conv. value: %4.3e.  BallGone: %d\n", missingCount, maxConv, ballGone);
        }
        else
            ballGone=0;
        
#if defined DEBUG && (DEBUG > 2)
        printf("Finding orientation markers...\n");
#endif
        if( !ballGone && orientBall && (orientationLocks!=numMarkers) ){
            markerLocalizer( &contoursFound, images, ball, markers, numMarkers, &numMarkersFound, params );            
            printf("OrientationLock falied,  orientationLocks = %d\n", orientationLocks);
        }
        
        
#if defined DEBUG && (DEBUG > 2)
        printf("numMarkersFound = %d\n", numMarkersFound);
        printf("Finding orientation...\n");
#endif
        //Have we found the markers?
        if(ballGone || !orientBall){
            for (int i=1; i<numMarkers; i++)
                markers[i].found=false;
        }
        else if (orientationLocks == numMarkers || numMarkersFound >= numMarkers ){
            orientationFinder( markers, images, &x_hatR_ijk /*observed variables*/, &y_measrd_xy, numMarkers, contoursFound, params,
                    &orientationLocks, &y_measrd_ijk);
            if (orientationLocks != numMarkers)
                orientationLocks=0;
        }        
        else{ //These trajectories move the ball around in order to find the three orientation markers. THEY NEED TO BE PROGRAMMED.
            printf("Warning, ball orientation is unknown. %d markers were found.\n", numMarkersFound);
            setpoint.pathNum=0;
            strcpy(setpoint.pathName, "findMarkers");
        }
        
        
#if defined DEBUG && (DEBUG > 1)
        printf("Max conv. value: %4.3e.\n", maxConv);
#endif       
        // Adjust for camera position and convert [px] to [m]
        cvAddWeighted( &y_measrd_xy,  1/calibration, cameraCenter, -1/calibration, 0, &y_measrdR_xy );
        
        
        //==============================================
        // Observer module that reconstructs the ball's state
        //==============================================
#if defined DEBUG && (DEBUG > 2)
        printf("Applying Kalman observer...\n");
#endif
        if (table.cameraHeight>0){        //Recalculate C_ball(x,u)
            double sX = table.cameraHeight/(table.cameraHeight - ball.dia/2);
            double sY = table.cameraHeight/(table.cameraHeight - ball.dia/2);
            
            cvmSet( kalman_system->measurement_matrix, 0, 0, sX);
            cvmSet( kalman_system->measurement_matrix, 1, 2, sY);
            //           printCvArr(kalman_ball->measurement_matrix);
//            printf("sX = %f, sy = %f\n", 1-fabs(sX), 1-fabs(sY));
        }
        
        // Adjust coordinate origin to camera reference plane
        cvmSet( &x_hatR_xy, 0, 0, cvmGet(&x_hatR_xy, 0, 0)-cvmGet(cameraCenter, 0, 0)/calibration);
        cvmSet( &x_hatR_xy, 2, 0, cvmGet(&x_hatR_xy, 2, 0)-cvmGet(cameraCenter, 1, 0)/calibration);

        kalmanCD_3_predict( &kalman_system, u_3 );
        
        if((keyPress == 'o') || (keyPress == 111)){ //Allows turning the observer on and off in order to ensure that the observer and model are correct. HERE THEY ARE NOT!!!
            observerOnOff=!observerOnOff;
            displayType=1 ;
        }
        
        if (missingCount==0 || observerOnOff){ //If we have found the ball, correct...
            
            gettimeofday(&params.tv_received, NULL);
            params.latency = params.tv_received.tv_sec + 1e-6*params.tv_received.tv_usec - params.trigger_time;
//          printf("Decalage est de %f ms\n", params.latency);
            
//            cvKalmanCorrect( kalman_system, y_measrdR );
            
            xi=cvmGet( &x_hatR_ijk, 0, 0 );
            yi=cvmGet( &x_hatR_ijk, 1, 0 );
            zi=cvmGet( &x_hatR_ijk, 2, 0 );
            xk=cvmGet( &x_hatR_ijk, 3, 0 );
            yk=cvmGet( &x_hatR_ijk, 4, 0 );
            zk=cvmGet( &x_hatR_ijk, 5, 0 );
            
            if (orientationLocks!=numMarkers){
                cvmSet( &y_measrdR_ijk, 0, 0, xi );
                cvmSet( &y_measrdR_ijk, 1, 0, yi );
                cvmSet( &y_measrdR_ijk, 2, 0, zk );
                cvmSet( &y_measrdR_ijk, 3, 0, yk );
            }
            
//                    printCvArr(kalman_orientation->gain);
//                    printCvArr(kalman_orientation->transition_matrix);
//                    printCvArr(kalman_orientation->state_pre);
//                    printCvArr(kalman_orientation->state_post);
//                    printCvArr(kalman_orientation->error_cov_pre);
//                    printCvArr(kalman_orientation->error_cov_post);
//                    cvWaitKey(0);
            
// //                cvKalmanCorrect( kalman_orientation, orientation_measrd );
            
            kalmanCD_3_correct( &kalman_system, y_measrdR ); //Perform Kalman filtering
   
            if (orientationLocks==numMarkers){
                //Rot is a rotation matrix
                cvmSet( Rot, 0, 0, xi );
                cvmSet( Rot, 0, 1, zk*xi-xk*zi );
                cvmSet( Rot, 0, 2, xk  );
                cvmSet( Rot, 1, 0, yi );
                cvmSet( Rot, 1, 1, yk*zi-yi*zk );
                cvmSet( Rot, 1, 2, yk  );
                cvmSet( Rot, 2, 0, zi );
                cvmSet( Rot, 2, 1, xk*yi-yk*xi );
                cvmSet( Rot, 2, 2, zk  );
//                    printCvArr(Rot);
                cvMulTransposed(Rot, tmp33, 1);
//                    printCvArr(tmp33);
                for (int i=0; i<numMarkers; i++){
                    cvMatMul(Rot, markers[i].rotationMat, tmp33);
//                    printCvArr(tmp33);
                    cvMatMul( tmp33, zVector, markers[i].spatialCoordsMat ); // Updates the approx 2-D location of the markers based on filtered data
//                    printCvArr(markers[i].spatialCoordsMat);
                }
//                    cvWaitKey(0);
            }
            
            
        }
        else{ //and if we haven't, trust the prediction.
            cvCopy( kalman_system->state_pre, kalman_system->state_post);
        }
        
        // Return coordinate origin to table reference plane
        cvmSet( &x_hatR_xy, 0, 0, cvmGet(&x_hatR_xy, 0, 0)+cvmGet(cameraCenter, 0, 0)/calibration);
        cvmSet( &x_hatR_xy, 2, 0, cvmGet(&x_hatR_xy, 2, 0)+cvmGet(cameraCenter, 1, 0)/calibration);
        
        // Convert [m] to [px]
        cvScale( &x_hatR_xy, &x_hat_xy, calibration );
        ball.pixCoords[0]=cvmGet(x_hat, 0, 0);
        ball.pixCoords[1]=cvmGet(x_hat, 2, 0);
        
//------------------------------BEGIN 3RD SANDBOX-------------------------//
        
//------------------------------END 3RD SANDBOX-------------------------//
        
        
        //==============================================
        // Trajectory module that generates the set point
        //==============================================
#if defined DEBUG && (DEBUG > 2)
        printf("Updating setpoint...\n");
#endif
        
        if((keyPress == 'p') | (keyPress == 112)){
            setpoint.pathNum++; //pathNum is the numerical representation of the chosen path. This advances the path to the next in the list
            setpoint.trajectoryStep=0; //Resets curve to beginning
            cvmSet( setpoint.setPoint, 1, 0, 0 ); //Resets setpoint velocity to 0
            cvmSet( setpoint.setPoint, 3, 0, 0 ); //Resets setpoint velocity to 0
            
            updateSetPoint( &setpoint, markers, ball, customTrajectory, numMarkers, runTime ); //Generates the new setpoint.
            
            // Output trajectory name to screen
            sprintf(ballLocText, setpoint.pathName);
            cvPutText( colorImage, ballLocText, cvPoint( 10, 30 ), &textFont , cvScalar(255, 255, 255) );
            cvShowImage( WINDOW_RUN, colorImage );
        }
        else
            updateSetPoint( &setpoint, markers, ball, customTrajectory, numMarkers, runTime ); //Generates the new setpoint.
        
        // Convert [px] to [m]
        cvScale( setpoint.setPoint, setpoint.setPointR, 1/calibration );
        
        
        //==============================================
        // Controller module that generates the table's inputs
        //==============================================
#if defined DEBUG && (DEBUG > 2)
        printf("Computing control...\n");
#endif
        //Compute error
        cvSub( setpoint.setPointR, &x_hatR_xy, setpoint.errMat );

        //Compute LQR correction
        cvMatMul( K_dlqr_3, setpoint.errMat, u_3 );
                        printf("x position integrator\n");

        //Compute integrator correction
        if ( ballGone == 0 ){
            //Integrator anti-windup. This keeps the integrator from running away on us.
            //THE UPPER SATURATION LIMITS ARE NOT EXACTLY CORRECT, BUT THEY DO IN A PINCH.
//        if (cvmGet(setpoint.integrationPositionErr, 0, 0)*setpoint.integrationPositionGain > sat ){
//            printf("Position integrator value = %f\n", cvNorm(setpoint.integrationPositionErr)*setpoint.integrationPositionGain);
//            cvNormalize(setpoint.integrationPositionErr, setpoint.integrationPositionErr, fabs(sat/setpoint.integrationPositionGain));
//            printCvArr(setpoint.integrationPositionErr);
//            printf("Position integrator value = %f\n", cvNorm(setpoint.integrationPositionErr)*setpoint.integrationPositionGain);
//        }
//        if (cvNorm(setpoint.integrationVelocityErr)*setpoint.integrationVelocityGain > fabs(sat) ){
//            cvNormalize(setpoint.integrationVelocityErr, setpoint.integrationVelocityErr, fabs(sat/setpoint.integrationVelocityGain));
//            printCvArr(setpoint.integrationVelocityErr);
//            printf("Velocity integrator value = %f\n", cvNorm(setpoint.integrationVelocityErr)*setpoint.integrationVelocityGain);
//        }
            
            tmpSat=cvmGet( setpoint.integrationPositionErr, 0, 0 )*setpoint.integrationPositionGain;
            if ( (tmpSat > sat) || (tmpSat < -sat) ){
                if (tmpSat > 0)
                    cvmSet( setpoint.integrationPositionErr, 0, 0, sat/setpoint.integrationPositionGain);
                else
                    cvmSet( setpoint.integrationPositionErr, 0, 0, -sat/setpoint.integrationPositionGain);
                printf("x position integrator\n");
                printCvArr(setpoint.integrationPositionErr);
            }
            tmpSat=cvmGet( setpoint.integrationPositionErr, 1, 0 )*setpoint.integrationPositionGain;
            if ( (tmpSat > sat) || (tmpSat < -sat) ){
                if (tmpSat > 0)
                    cvmSet( setpoint.integrationPositionErr, 1, 0, sat/setpoint.integrationPositionGain);
                else
                    cvmSet( setpoint.integrationPositionErr, 1, 0, -sat/setpoint.integrationPositionGain);
                printf("y position integrator\n");
                printCvArr(setpoint.integrationPositionErr);
            }
            tmpSat=cvmGet( setpoint.integrationVelocityErr, 0, 0 )*setpoint.integrationVelocityGain;
            if ( (tmpSat > sat) || (tmpSat < -sat) ){
                if (tmpSat > 0)
                    cvmSet( setpoint.integrationVelocityErr, 0, 0, sat/setpoint.integrationVelocityGain);
                else
                    cvmSet( setpoint.integrationVelocityErr, 0, 0, -sat/setpoint.integrationVelocityGain);
                printf("x velocity integrator\n");
                printCvArr(setpoint.integrationVelocityErr);
            }
            tmpSat=cvmGet( setpoint.integrationVelocityErr, 1, 0 )*setpoint.integrationVelocityGain;
            if ( (tmpSat > sat) || (tmpSat < -sat) ){
                if (tmpSat > 0)
                    cvmSet( setpoint.integrationVelocityErr, 1, 0, sat/setpoint.integrationVelocityGain);
                else
                    cvmSet( setpoint.integrationVelocityErr, 1, 0, -sat/setpoint.integrationVelocityGain);
                printf("y velocity integrator\n");
                printCvArr(setpoint.integrationVelocityErr);
            }
/* INTEGRATOR TURNED OFF UNTIL I DECIDE HOW TO HANDLE IT WITH 3 INPUTS            
            cvScaleAdd(setpoint.integrationPositionErr, cvScalar(setpoint.integrationPositionGain), u_3, u_3);
            cvScaleAdd(setpoint.integrationVelocityErr, cvScalar(setpoint.integrationVelocityGain), u_3, u_3);
*/
            
        }
        
        // Actuator saturation
        // THE SATURATION VALUE SHOULD BE DEFINED IN A USER CONFIGURABLE FILE.
        // THE UPPER AND LOWER VALUES SHOULD BE DEFINED INDIVIDUALLY FOR EACH ACTUATOR
        tmpSat=cvmGet( u_3, 0, 0 );
        if ( (tmpSat > sat) || (tmpSat < -sat) ){
            if (tmpSat > 0)
                cvmSet( u_3, 0, 0, sat);
            else
                cvmSet( u_3, 0, 0, -sat);
        }
        tmpSat=cvmGet( u_3, 1, 0 );
        if ( (tmpSat > sat) || (tmpSat < -sat) ){
            if (tmpSat > 0)
                cvmSet( u_3, 1, 0, sat);
            else
                cvmSet( u_3, 1, 0, -sat);
        }
        
        //==============================================
        // cntrOutput(); // Module that outputs the control signal to the actuators
        // This routine MUST shut itself down if we can't find the ball.
        //==============================================
#if defined DEBUG && (DEBUG > 1)
        printf("Outputting control...\n");
#endif        
        if (ballGone == 0 ){
            sendOutput( fd, u_3 );
        }
        
        
        //==============================================
        // Local data output module
        // Turn off display by pressing '0'
        // Display image by pressing '1'
        // Display binarised image by pressing '2'
        // Display edge extraction image by pressing '3'
        // Display convolution image by pressing '4'
        // Save image by pressing 's'
        // Display console information 'c'
        //==============================================
        if( displayConsole ){ //Outputs data to console
            printf("u= %4.4f %4.4f , ", cvmGet( u_3, 0, 0 ), cvmGet( u_3, 1, 0 ));
            printf("x_hatR= %4.4f %4.4f %4.4f %4.4f  \n", cvmGet( x_hatR, 0, 0 ), cvmGet( x_hatR, 1, 0 ), cvmGet( x_hatR, 2, 0 ), cvmGet( x_hatR, 3, 0 ));
            printf("  y_measrd= %4.4f %4.4f , ", cvmGet( y_measrd, 0, 0 ), cvmGet( y_measrd, 1, 0 ));
            printf("Set point = %5.1f x %5.1f\n", cvmGet(setpoint.setPoint, 0, 0), cvmGet(setpoint.setPoint, 2, 0) );
        }
        
        if (dataOut){ // Logs data to text file
                fprintf(dataOut, "%5.5f, %5.5f, %5.5f, %5.5f, \t", cvmGet( x_hatR, 0, 0 ), cvmGet( x_hatR, 1, 0 ), cvmGet( x_hatR, 2, 0 ), cvmGet( x_hatR, 3, 0 ));
                fprintf(dataOut, "%5.1f, %5.1f, %5.1f, %5.1f, \t", cvmGet( setpoint.setPoint, 0, 0 ), cvmGet( setpoint.setPoint, 1, 0 ), cvmGet( setpoint.setPoint, 2, 0 ), cvmGet( setpoint.setPoint, 3, 0 ));
                fprintf(dataOut, "%6.2f, %6.2f, \t", cvmGet( y_measrd, 0, 0 ), cvmGet( y_measrd, 1, 0 ));
                fprintf(dataOut, "%f, %f,\t", cvmGet( u_3, 0, 0 ), cvmGet( u_3, 1, 0 ));
                fprintf(dataOut, "%f, %f\n", cvmGet( setpoint.integrationPositionErr, 0, 0 ), cvmGet( setpoint.integrationPositionErr, 1, 0 ));
                fprintf(dataOut, "%f, %f\n", cvmGet( setpoint.integrationVelocityErr, 0, 0 ), cvmGet( setpoint.integrationVelocityErr, 1, 0 ));                
        }
        
        if ( keyHold=='1' || displayType==1 ){
            sprintf(ballLocText, "%3.0f x %3.0f", round(cvmGet(y_measrd, 0, 0)), round(cvmGet(y_measrd, 1, 0)));
            sprintf(fpsText, "fps: %4.2f, Computation time: %4.2f", fpsRate, computationTime);
            cvPutText( colorImage, fpsText, cvPoint( 10, colorImage->height -10 ), &textFont , cvScalar(255, 255, 255) );
            cvPutText( colorImage, ballLocText, cvPoint( images.cropRect.x, images.cropRect.y-3 ), &textFont , cvScalar(255, 255, 255) );
            cvRectangle( colorImage, cvPoint( images.cropRect.x, images.cropRect.y ), cvPoint( images.cropRect.x+images.cropRect.width, images.cropRect.y+images.cropRect.height ), cvScalar(255, 255, 255), 1); //Shows cropping frame
            cvCircle( colorImage, cvPoint(cvRound(cvmGet(y_measrd, 0, 0)), cvRound(cvmGet(y_measrd, 1, 0))), (maskImage->width-1)/2, cvScalar(255, 255, 255), 1 ); //Shows where the ball image was found.

            cvCircle( colorImage, cvPoint(cvRound(cvmGet(setpoint.setPoint, 0, 0)), cvRound(cvmGet(setpoint.setPoint, 2, 0))), 2, CV_RGB(255, 100, 0), 1 ); //Shows the current setpoint position
            cvLine( colorImage, cvPoint(cvRound(cvmGet(setpoint.setPoint, 0, 0)), cvRound(cvmGet(setpoint.setPoint, 2, 0))), cvPoint(cvRound(cvmGet(setpoint.setPoint, 0, 0)-cvmGet(setpoint.setPoint, 1, 0)), cvRound(cvmGet(setpoint.setPoint, 2, 0)-cvmGet(setpoint.setPoint, 3, 0))), CV_RGB(255, 0, 0), 1 );  //Shows the current setpoint velocity
            cvCircle( colorImage, cvPoint(cvRound(cvmGet(x_hat, 0, 0)), cvRound(cvmGet(x_hat, 2, 0))), 2, CV_RGB(50, 0, 255), 1 ); //Shows the observer's position estimation
            cvLine( colorImage, cvPoint(cvRound(cvmGet(x_hat, 0, 0)), cvRound(cvmGet(x_hat, 2, 0))), cvPoint(cvRound(cvmGet(x_hat, 0, 0)-cvmGet(x_hat, 1, 0)), cvRound(cvmGet(x_hat, 2, 0)-cvmGet(x_hat, 3, 0))), CV_RGB(0, 0, 255), 1 );  //Shows the observer's velocity estimation
            
            //This section visualizes the ball's orientation.
            if (orientBall){
                double scaleAxes=calibration*table.ballDia/2;
                
                CvPoint ballCenter=cvPoint(cvRound(cvmGet(x_hat, 0, 0)), cvRound(cvmGet(x_hat, 2, 0)));
                cvLine( colorImage, ballCenter, cvPoint(cvRound(ballCenter.x+xi*scaleAxes), cvRound(ballCenter.y+yi*scaleAxes)), CV_RGB( 255, 100, 100 ), 1 ); //Draw x-axis in red
                cvLine( colorImage, ballCenter, cvPoint(cvRound(ballCenter.x-(yk*zi-yi*zk)*scaleAxes), cvRound(ballCenter.y-(xi*zk-xk*zi)*scaleAxes)), CV_RGB( 150, 255, 150 ), 1 ); //Draw y-axis in green
                cvLine( colorImage, ballCenter, cvPoint(cvRound(ballCenter.x+xk*scaleAxes), cvRound(ballCenter.y+yk*scaleAxes)), CV_RGB( 100, 100, 255 ), 1 ); //Draw z-axis in blue
            }
            cvShowImage( WINDOW_RUN, colorImage );
        }
        else if ( keyHold=='2' || displayType==2 ){
            cvShowImage( WINDOW_RUN, images.bwImage );
        }
        else if ( keyHold=='3' || displayType==4 ){
            cvShowImage( WINDOW_RUN, edgeImageOUT );
        }
        else if ( keyHold=='4' || displayType==5 ){
            if ( std::max(images.convMat->cols, images.convMat->rows) < 200 ) // imagesc2 is slow, so have to limit its use.
                imagesc( WINDOW_ADJUST, images.convMat );
            else{
                cvPutText( edgeImageOUT, "TOO HIGH A RESOLUTION.", cvPoint( 10, 30 ), &textFont , cvScalar(255, 255, 255) );
                cvPutText( edgeImageOUT, "REDUCE RESOLUTION IN", cvPoint( 10, 55 ), &textFont , cvScalar(255, 255, 255) );
                cvPutText( edgeImageOUT, "ORDER TO VISUALIZE CONVOLUTION.", cvPoint( 10, 80 ), &textFont , cvScalar(255, 255, 255) );
                cvShowImage( WINDOW_ADJUST, edgeImageOUT );
            }
        }
        
        //==============================================
        // Remote data output module
        //==============================================
        //	if ( fdOUT != -1 )
        //		fifoCvArrOut( images.convMat, fdOUT);
        
        //==============================================
        // End of loop
        //==============================================
        
        // Loop garbage collection
        cvReleaseMat( &images.convMat );
        cvReleaseImage( &edgeImageOUT );
        
        //==================================
        // Exit program by pressing ESC key.
        //==================================
        keyPress = ( cvWaitKey(2) & 255 );
        if (keyPress =='r'){ //This resets the crop. Handy if the computer has zoomed in on the wrong object.
            missingCount=1;
            ballGone=1;
        }
        else if ((keyPress == 's') || (keyPress == 115))
            cvSaveImage(filename, colorImage), printf( "Image saved\n" );
        else if ((keyPress == 'c') || (keyPress == 99))
            displayConsole= !displayConsole;
        else if ((keyPress !=-1) && (keyPress !=255))
            keyHold=keyPress;
        
        //Output time
        toc = cvGetTickCount();
        computationTime=(double)(toc-tic_speed)*1000/ticRate;
        if( displayConsole )
            printf("    Computation time: %3.3lf ms. FPS: %lf fps\n", computationTime, fpsRate );
        if (computationTime>delT*1000){
            numOverruns++;
            printf("ERROR!!! Overshot sample time by %3.3lf ms!!!\n", computationTime-delT*1000 );
        }
        
    }
    
    //Write saved states output to file.
    CvFileStorage* stateOutput = cvOpenFileStorage( "bandpSavedState.yml", 0, CV_STORAGE_WRITE);
    cvWrite( stateOutput, "savedPast", savedPast ); //This write out has been superceeded by an fprintf function earlier in the code.
    cvWrite( stateOutput, "integrationPositionErr ", setpoint.integrationPositionErr ); //This line is just here while awaiting a better place to put it. This information 
    cvWrite( stateOutput, "integrationVelocityErr", setpoint.integrationVelocityErr );  //doesn't belong in SavedState, and is in fact read from UserVariables.
    cvReleaseFileStorage( &stateOutput );
   
    
    //=====================================================================================
    // Garbage collection
    //=====================================================================================
    closePort( fd );
    if (dataOut) //Close file
        fclose(dataOut);
    releaseCamera( camFeed );
    cvDestroyWindow( WINDOW_RUN );
    cvReleaseImage( &maskImage );
    cvReleaseMat( &x_hat );
    cvReleaseMat( &x_hatR );
    cvReleaseMat( &y_measrd );
    cvReleaseMat( &y_measrdR );
//    cvReleaseMat( &orientation_measrd );
    cvReleaseMat( &u_3 );
//    cvReleaseMat( ??? ); //NEED TO GO THROUGH AND RELEASE all THE MATS DEFINED INSIDE THE STRUCTURES!!!
    cvReleaseMat( &cameraGammaLUT );
    cvReleaseMat( &advanceTime );
    cvReleaseMat( &savedPast );
//    cvReleaseMat( &ballOrientation );
//    cvReleaseMat( &ballOrientationA );
//    cvReleaseMat( &ballOrientationQ );
//    cvReleaseMat( &ballOrientationR );
    cvReleaseMat( &zVector );
    cvReleaseMat( &Rot );
    cvReleaseMemStorage(&contoursFoundStorage);
    if ( orientBall )// if orientBall is off, then the memory was never allocated
        free( markers );

#ifdef GUI
    bandpGUILink.closesocket();
#endif
    
    printf("\n\nOvershot sample period %d times.\n", numOverruns);
    
    return 1;
}
